/**
 *             _..--""-.                  .-""--.._
           _.-'         \ __...----...__ /         '-._
         .'      .:::...,'              ',...:::.      '.
        (     .'``'''::;                  ;::'''``'.     )
         \             '-)              (-'             /
          \             /                \             /
           \          .'.-.            .-.'.          /
            \         | \0|            |0/ |         /
            |          \  |   .-==-.   |  /          |
             \          `/`;          ;`\`          /
              '.._      (_ |  .-==-.  | _)      _..'
                  `"`"-`/ `/'        '\` \`-"`"`
                       / /`;   .==.   ;`\ \
                 .---./_/   \  .==.  /   \ \
                / '.    `-.__)       |    `"
               | =(`-.        '==.   ;
                \  '. `-.           /
                 \_:_)   `"--.....-'

                 file: gripper_publisher.h
                 description: state publisher for the urdf gripper.
                 authors: Michiel and Peter
 */
#ifndef GRIPPER_PUBLISHER_H__
#define GRIPPER_PUBLISHER_H__

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Bool.h>
#include "gripper_defines.h"
#include "topics.h"

class GripperPublisher
{
protected:
	ros::NodeHandle nh_;
	ros::Subscriber sens_;
	ros::Publisher joint_;
	ros::Subscriber motor_state_sub_;
	sensor_msgs::JointState joint_state_;

	double distance;	// distance in dm
	double rotation;	// rotation
	double rotation_top;	// rotation finger tops
	int motor_state;	// OPEN or CLOSED
public:
	GripperPublisher() : distance(-1), rotation(GripperDefines::ROTATION_MIDDLE_MIN), rotation_top(GripperDefines::ROTATION_TOP_MIN), motor_state(GripperDefines::OPEN)
	{
	}

	~GripperPublisher()
	{
		nh_.shutdown();
	}

	/**
	 * Sensor callback.
	 * @param	msg	distance from sensor in cm.
	 */
	void sensorCallback(const std_msgs::Int32::ConstPtr &msg);

	/**
	 * Motor callback.
	 * @param	state Int32, OPEN or CLOSED.
	 */
	void motorCallback(const std_msgs::Int32::ConstPtr &state);

	/**
	 * Init gripper publisher, set joints etc.
	 */
	void init();

	/**
	 * Update state values.
	 */
	void spin();

	/**
	 * Close the gripper.
	 * @return	boolean		true if closed, otherwise false.
	 */
	bool closeGripper();

	/**
	 * Open the gripper.
	 * @return	boolean		true if open, otherwise false.
	 */
	bool openGripper();
};

#endif
